#include "EOMData.h"
#include "MatrixClasses.h"
#include "Transform.h"
#include "FastOps.h"

using namespace RSIM;

///////////////////////////////////////////////////////

EOMData::EOMData():	dMk1_(NULL),Jk1_(NULL),Ak1_(NULL),
                        Fkc1_(NULL),dFk1_(NULL){
}

///////////////////////////////////////////////////////

void EOMData::clear(){
        delete dMk1_;	dMk1_ = NULL;
        delete Jk1_;	Jk1_ = NULL;
        delete Ak1_;	Ak1_ = NULL;
        delete Fkc1_;	Fkc1_= NULL;
        delete dFk1_;	dFk1_ = NULL;
}

///////////////////////////////////////////////////////

EOMData::~EOMData(){
        delete dMk1_;
        delete Jk1_;
        delete Ak1_;
        delete Fkc1_;
        delete dFk1_;
}

///////////////////////////////////////////////////////
// This function can be made faster. However, this is invoked
// just once at the start of the simulation so, the excess computation
// will not add to overall simulation cost. 
void EOMData::initEOM(const double& Mass,const Mat3x3& Inertia, const Transform& T_BF){ // sFlag
        this->dMk1_ = new Mat6x6;
        this->Jk1_ = new Mat3x3;
        this->dFk1_ = new Vect6(0.0);
        
        this->Fkc1_ = new Vect6(0.0);
        this->Ak1_ = new Vect6(0.0);
        
        Mat3x3 rkx(0.0);
        Mat3x3 m3x3(0.0);
        m3x3.Diagonal(Mass,Mass,Mass);
        const InvTransform& T_FB = InvTransform(T_BF);
        
        // Form matrix r^k x
        FastOps::formCrossProdMat(T_FB.P().Ptr(), rkx.wPtr());
        
        // Jk1 = Inertia - mass * [rx][rx]
        // Inertia and rx are both directly written in frame F
        *Jk1_ = T_FB.R()*Inertia*T_BF.R() - Mass*rkx*rkx;
        
        dMk1_->copyMat3x3AtIndex(0,*Jk1_);
        dMk1_->copyMat3x3AtIndex(1,Mass*rkx);
        dMk1_->copyMat3x3AtIndex(2,-Mass*rkx);
        dMk1_->copyMat3x3AtIndex(3,m3x3);
}

///////////////////////////////////////////////////////